#include "mine_fly_control.h"
#include "motors.h"
#include <semphr.h>
#include <stdio.h>

#define USE_MINE_MOTORS

#define bit0 (1 << 0) // 事件组标记位

// 起飞状态
static u8 mine_fly_control_fly_state = 0;

TaskHandle_t mine_fly_control_task_handler;
EventGroupHandle_t eventGroupsHandler; // 事件组 句柄对象

SemaphoreHandle_t sema_handler; // 信号量

static float mine_fly_ctrl_speed = 0.2f;
u8 mineFlyStateChanged = 0;

void mine_fly_control_init()
{

#ifdef USE_MINE_MOTORS
    mine_motors_pwm_init();
#else
    motorsInit();
#endif
}
float mine_fly_control_pwd = 0.0f;
void mine_fly_control_task(void *pvParameters)
{
    u32 tick = 0;
    u32 lastWakeTime = getSysTickCnt();

    while (1)
    {
        vTaskDelayUntil(&lastWakeTime, 1u);
        // k
        if (tick % 2 == 0)
        {
            mine_fly_control_pwm_set(mine_fly_control_pwd);
            // if (mineFlyStateChanged)
            // {

            // mineFlyStateChanged = 0;
            // u8 fly_state = mine_fly_control_get_fly_state();
            // printf("mine_fly_control_fly_state = %d\n", mine_fly_control_fly_state);
            // switch (fly_state)
            // {
            // case 0:
            //     mine_fly_control_land();
            //     break;
            // case 1:
            //     mine_fly_control_flight();
            //     break;
            // case 2:
            //     mine_fly_control_speed_up();
            //     break;
            // case 3:
            //     mine_fly_control_speed_down();
            //     break;
            // }
            // }
        }
        tick++;
        // vTaskDelay(80);
    }
}

uint8_t mine_fly_control_get_fly_state()
{
    return mine_fly_control_fly_state;
}

void mine_fly_control_set_fly_state(uint8_t fly_state)
{
    printf("mine_fly_control_set_fly_state::%d\n", fly_state);
    mineFlyStateChanged = 1;
    mine_fly_control_fly_state = fly_state;

    switch (fly_state)
    {
    case 0:
        // mine_fly_control_land();
        mine_fly_control_pwd = 0.0f;
        break;
    case 1:
        // mine_fly_control_flight();
        mine_fly_control_pwd = 0.1f;

        break;
    case 2:
        // mine_fly_control_speed_up();
        mine_fly_control_pwd += 0.1f;
        if (mine_fly_control_pwd > 1.0f)
        {
            mine_fly_control_pwd = 1.0f;
        }
        break;
    case 3:
        // mine_fly_control_speed_down();
        mine_fly_control_pwd -= 0.1f;
        if (mine_fly_control_pwd < 0.0f)
        {
            mine_fly_control_pwd = 0.0f;
        }
        break;
    }
}

/**
 * 加油
 */
void mine_fly_control_speed_up()
{
    // printf("mine_fly_control_speed_up:加速\n");

    mine_fly_ctrl_speed += 0.1f;
    if (mine_fly_ctrl_speed > 1.0f)
    {
        mine_fly_ctrl_speed = 1.0f;
    }
    u16 pwd = (u16)(mine_fly_ctrl_speed * (1 << 16));

#ifdef USE_MINE_MOTORS
    mine_motors_pwm_set(0, pwd);
    mine_motors_pwm_set(1, pwd);
    mine_motors_pwm_set(2, pwd);
    mine_motors_pwm_set(3, pwd);
#else
    motorsSetRatio(0, pwd);
    motorsSetRatio(1, pwd);
    motorsSetRatio(2, pwd);
    motorsSetRatio(3, pwd);
#endif
}

/**
 * jiansu
 */
void mine_fly_control_speed_down()
{
    // printf("mine_fly_control_speed_down:减速\n");
    mine_fly_ctrl_speed -= 0.1f;
    if (mine_fly_ctrl_speed < 0.0f)
    {
        mine_fly_ctrl_speed = 0.0f;
    }
    u16 pwd = (u16)(mine_fly_ctrl_speed * (1 << 16));

#ifdef USE_MINE_MOTORS
    mine_motors_pwm_set(0, pwd);
    mine_motors_pwm_set(1, pwd);
    mine_motors_pwm_set(2, pwd);
    mine_motors_pwm_set(3, pwd);
#else
    motorsSetRatio(0, pwd);
    motorsSetRatio(1, pwd);
    motorsSetRatio(2, pwd);
    motorsSetRatio(3, pwd);

#endif
}
/**
 * 起飞
 */
void mine_fly_control_flight()
{
    // printf("mine_fly_control_flight:起飞\n");
    mine_fly_ctrl_speed = 0.2f;

#ifdef USE_MINE_MOTORS
    mine_motors_pwm_set(0, MOTORS_TEST_RATIO);
    mine_motors_pwm_set(1, MOTORS_TEST_RATIO);
    mine_motors_pwm_set(2, MOTORS_TEST_RATIO);
    mine_motors_pwm_set(3, MOTORS_TEST_RATIO);
#else
    motorsSetRatio(0, MOTORS_TEST_RATIO);
    motorsSetRatio(1, MOTORS_TEST_RATIO);
    motorsSetRatio(2, MOTORS_TEST_RATIO);
    motorsSetRatio(3, MOTORS_TEST_RATIO);
#endif
}

/**
 * 降落
 */
void mine_fly_control_land()
{
    /// printf("mine_fly_control_land:降落\n");

    mine_fly_ctrl_speed = 0.0f;

#ifdef USE_MINE_MOTORS
    mine_motors_pwm_set(0, 0U);
    mine_motors_pwm_set(1, 0U);
    mine_motors_pwm_set(2, 0U);
    mine_motors_pwm_set(3, 0U);
#else
    motorsSetRatio(0, 0);
    motorsSetRatio(1, 0);
    motorsSetRatio(2, 0);
    motorsSetRatio(3, 0);
#endif
}

void mine_fly_control_pwm_set(float duty)
{
    if (duty > 1.0f)
    {
        duty = 1.0f;
    }
    u16 pwd = (u16)(duty * (1 << 16));

#ifdef USE_MINE_MOTORS
    mine_motors_pwm_set(0, pwd);
    mine_motors_pwm_set(1, pwd);
    mine_motors_pwm_set(2, pwd);
    mine_motors_pwm_set(3, pwd);
#else
    motorsSetRatio(0, pwd);
    motorsSetRatio(1, pwd);
    motorsSetRatio(2, pwd);
    motorsSetRatio(3, pwd);
#endif
}
